#include "RespiratoryRateCalculator.h"
#include <cmath>
#include <iostream>
#include "./app/AsyncLogger.h"

// Initialize static constants
const double RespiratoryRateCalculator::refractoryPeriodSec = 0.4;

// Singleton instance
RespiratoryRateCalculator& RespiratoryRateCalculator::getInstance() {
    static RespiratoryRateCalculator instance;
    return instance;
}

// Constructor
RespiratoryRateCalculator::RespiratoryRateCalculator() 
    : refractoryPeriodSamples(static_cast<int>(FR * refractoryPeriodSec)),
      lastPeakIndex(-refractoryPeriodSamples),
      resp(0),
      previousRate(12) {
}

// Add new ECG data and process it
void RespiratoryRateCalculator::addData(double ecgValue, int sampleIndex) {
    ecgBuffer.push_back(ecgValue);
    
    // When we have enough data (2 seconds), detect R-peaks
    if (ecgBuffer.size() >= FR * 2) {
        std::vector<NumberPair> peaks = findRPeaks(ecgBuffer);
        for (size_t i = 0; i < peaks.size(); i++)
        {
            /* code */
            //asyncPrint("R-peak at index %d with value %.2f\r\n", peaks[i].intValue, peaks[i].doubleValue);
        }
        
        
        // Process detected R-peaks
        for (const auto& pair : peaks) {
            int peakIndex = pair.intValue;
            double peakValue = pair.doubleValue;
            
            int absolutePeakIndex = sampleIndex - static_cast<int>(ecgBuffer.size()) + peakIndex;
            
            if (absolutePeakIndex > lastPeakIndex + refractoryPeriodSamples) {
                rPeakTimestamps.push(absolutePeakIndex);
                rPeakTimestampsValues.push(peakValue);
                lastPeakIndex = absolutePeakIndex;
                
                // When we have enough R-peaks, calculate respiratory rate
                if (rPeakTimestamps.size() >= requiredPeaks) {
                    resp = findRespPeaks(rPeakTimestamps, rPeakTimestampsValues);
                    for (int i = 0; i < 4; ++i) {
                        rPeakTimestamps.pop();
                        rPeakTimestampsValues.pop();   
                    }
                }
            }
        }
        // Keep only the last 1 second of data to avoid reprocessing
        if (ecgBuffer.size() > FR) {
            ecgBuffer = std::vector<double>(
                ecgBuffer.end() - FR, 
                ecgBuffer.end()
            );
        }
    }
}

// Find R-peaks in ECG data
std::vector<RespiratoryRateCalculator::NumberPair> 
RespiratoryRateCalculator::findRPeaks(const std::vector<double>& ecgData) {
    std::vector<NumberPair> rPeakIndices;
    
    if (ecgData.size() < 3) {
        return rPeakIndices;
    }
    
    // Calculate dynamic threshold
    double threshold = 0.6 * getMaxValue(ecgData);
    //asyncPrint("threshold = %.2f\r\n", threshold);
    
    // Find peaks above threshold
    for (size_t i = 1; i < ecgData.size() - 1; i++) {
        double current = ecgData[i];
        double prev = ecgData[i - 1];
        double next = ecgData[i + 1];
        
        // Simple peak detection
        if (current > prev && current > next && current > threshold) {
            // Check refractory period
            if (rPeakIndices.empty() || 
            ( 
                (static_cast<int>(i) - rPeakIndices.back().intValue)>=60 &&
                (static_cast<int>(i) - rPeakIndices.back().intValue) <= 392
            ) ) {
                rPeakIndices.emplace_back(static_cast<int>(i), ecgData[i]);
                // Adjust threshold dynamically
                threshold = 0.7 * threshold + 0.3 * current;
            }
        }
    }
    
    if (rPeakIndices.size() < 2) {
        //asyncPrint("R-peak count insufficient for heart rate\r\n");
    }
    
    // Calculate RR intervals (in samples)
    double totalRRInterval = 0;
    int RIntervalCount = 0;
    for (size_t i = 1; i < rPeakIndices.size(); i++) {
        int rrInterval = rPeakIndices[i].intValue - rPeakIndices[i-1].intValue;
        //asyncPrint("rrInterval %d = %d\r\n",i, rrInterval);
        totalRRInterval += rrInterval;
        RIntervalCount++;
    }
    
    double avgRRInterval = totalRRInterval / (RIntervalCount);
    double value = (60.0 * FR) / avgRRInterval;
    //asyncPrint("heart rate: %.2f\r\n",value);
    heartRate = value;
    
    return rPeakIndices;
}

// Calculate respiratory rate from R-peak data
double RespiratoryRateCalculator::findRespPeaks(const std::queue<int>& queue1, 
                                              const std::queue<double>& queue2) {
    std::vector<int> rPeakIndices;
    std::vector<double> rPeakValues;
    
    // Copy queues to vectors for processing
    std::queue<int> tempQ1 = queue1;
    std::queue<double> tempQ2 = queue2;
    
    while (!tempQ1.empty()) {
        rPeakIndices.push_back(tempQ1.front());
        rPeakValues.push_back(tempQ2.front());
        tempQ1.pop();
        tempQ2.pop();
    }
    
    std::vector<int> rPeakResult;
    
    // Calculate dynamic threshold
    double threshold = 0.6 * getMaxValue(rPeakValues);
    
    // Find peaks above threshold
    for (size_t i = 1; i < rPeakValues.size() - 1; i++) {
        double current = rPeakValues[i];
        double prev = rPeakValues[i - 1];
        double next = rPeakValues[i + 1];
        
        // Simple peak detection
        if (current > prev && current > next && current > threshold) {
            rPeakResult.push_back(rPeakIndices[i]);
            // Adjust threshold dynamically
            threshold = 0.7 * threshold + 0.3 * current;
        }
    }
    
    if (rPeakResult.size() < 2) {
        //asyncPrint("R-peak count insufficient for respiratory\r\n");
        return resp; // Return previous value if we can't calculate
    }
    
    // Calculate RR intervals (in samples)
    double totalRRInterval = 0;
    for (size_t i = 1; i < rPeakResult.size(); i++) {
        int rrInterval = rPeakResult[i] - rPeakResult[i-1];
        totalRRInterval += rrInterval;
    }
    
    double avgRRInterval = totalRRInterval / (rPeakResult.size() - 1);
    double calculatedResp = (60.0 * FR) / avgRRInterval;
    
    double value = applyPhysiologicalConstraints(calculatedResp);
    //asyncPrint("respiratory rate: %.2f\r\n", value);
    return value;
}

// Get maximum value in ECG data
double RespiratoryRateCalculator::getMaxValue(const std::vector<double>& ecgData) const {
    if (ecgData.empty()) return 0.0;
    
    double max = std::numeric_limits<double>::lowest();
    for (double value : ecgData) {
        if (value > max) {
            max = value;
        }
    }
    return max;
}

// Apply physiological constraints to respiratory rate
double RespiratoryRateCalculator::applyPhysiologicalConstraints(double rate) {
    const double MIN_RESPIRATORY_RATE = 6.0;   // Minimum respiratory rate for adults at rest
    const double MAX_RESPIRATORY_RATE = 24.0;  // Maximum respiratory rate for adults at rest
    const double MAX_CHANGE = 2.0;             // Maximum change between measurements
    
    // Apply physiological range constraints
    double constrainedRate = std::max(MIN_RESPIRATORY_RATE, std::min(MAX_RESPIRATORY_RATE, rate));
    
    // Apply rate of change constraints
    if (previousRate >= MIN_RESPIRATORY_RATE && previousRate <= MAX_RESPIRATORY_RATE) {
        double lowerBound = std::max(MIN_RESPIRATORY_RATE, previousRate - MAX_CHANGE);
        double upperBound = std::min(MAX_RESPIRATORY_RATE, previousRate + MAX_CHANGE);
        constrainedRate = std::max(lowerBound, std::min(upperBound, constrainedRate));
        previousRate = constrainedRate;
    }
    
    return constrainedRate;
}